#include "ir.h"
#include "log.h"
#include "iictool.h"

#include <algorithm>
#include <string>
#include "time.h"

        Ir::Ir()
        {
            Log *logger_ = new Log("Sensor Object"); // aanmaken van object, moet gedestroyed worden
            logger_->stuur(5, "nieuw object aangemaakt");
        }

       /* Ir::~Ir()
        {
            destroy logger;
        }
        */
        int Ir::getAngle()
        {
            int distA,distB,distK;

            distA=getDistance(000);
            distB=getDistance(001);

            if(distA==distB)                        //rechtdoor rijden
            {
                distK=distA;
                angle_=0;
            }
            else if(distA<distB)
            {
                distK=distA;       //naar links bijsturen
                angle_=-90;
            }
            else
            {
                distK=distB;                       //naar rechts bijsturen
                angle_=90;
            }

            return this->angle_;
        }

        void Ir::setAngle(int angle)
        {
            this->angle_=angle;
        }

        int Ir::getDistance(int id)
        {
            int samp[5];
            int ADCin_;

            Iictool *i2c = new Iictool("2");   //i2c bus 2 gebruiken

            i2c->iicSet("004","01000000","10000000");          //"chip_address","data_address","value" :Ai0 inschakelen
            i2c->iicSet("004","01000000","00000000");          //2de byte is 0
            //wait(50);
            usleep(50000);

            for(int i=0;i<6;i++)
            {
                    //wait(5);          //5ms wachten
                    usleep(5000);
                    samp[i]=i2c->iicGet("000","1001000");   //"chip_address","data_address" :Ai0 uitlezen
            }
            sort(samp,samp+5);
            ADCin_=samp[2];

            distance_=omrekenen(ADCin_);
            //omrekening van ingelezen waarden naar 3cm tot 30cm
            return this->distance_;
        }

        void Ir::setDistance(int distance)
        {
            this->distance_=distance;
        }

       int Ir::omrekenen(int afst)
        {
            int afstCm;

            if(afst<=0x0A)                      afstCm=0x1E;        //afstand 30cm of meer
            else if(afst>0x0A && afst<=0x19)    afstCm=0x14;        //afstand 20cm
            else if(afst>0x19 && afst<=0x2E)    afstCm=0x0F;        //afstand 15cm
            else if(afst>0x2E && afst<=0x3D)    afstCm=0x0A;        //afstand 10cm
            else if(afst>0x3D && afst<=0x57)    afstCm=0x08;        //afstand 8cm
            else if(afst>0x57 && afst<=0x7A)    afstCm=0x05;        //afstand 5cm
            else if(afst>0x7A && afst<=0xA3)    afstCm=0x03;        //afstand 3cm
            else                                afstCm=0x00;        //delault

            return afstCm;
        }

    //
    //eerst sensor aanzetten

    //50ms wachten

    //gemeten waarde uitlezen

    //waarde moet worden opgeslagen
    //5 waardes inlezen en de mediaan van de waardes neemen
    //dit omrekenen naar gekende waarde
    //en dit returnen
    //tussen 2 meetingen 5ms wachten

